#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <chrono>

class FramerateMonitor : public rclcpp::Node
{
public:
    FramerateMonitor() : Node("framerate_monitor")
    {
        // 获取参数
        this->declare_parameter("image_topic", "/left_cam/image_raw");
        std::string image_topic = this->get_parameter("image_topic").as_string();
        
        // 创建订阅者
        subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
            image_topic, 10, 
            std::bind(&FramerateMonitor::image_callback, this, std::placeholders::_1));
        
        // 初始化计时器和帧计数
        last_time_ = this->now();
        frame_count_ = 0;
        
        // 创建定时器，每秒计算一次帧率
        timer_ = this->create_wall_timer(
            std::chrono::seconds(1),
            std::bind(&FramerateMonitor::timer_callback, this));
            
        RCLCPP_INFO(this->get_logger(), "帧率监控节点已启动，订阅话题: %s", image_topic.c_str());
    }

private:
    void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
    {
        // 每收到一帧图像，帧计数加1
        frame_count_++;
    }
    
    void timer_callback()
    {
        // 计算当前时间与上次时间的差值
        auto current_time = this->now();
        auto elapsed = current_time - last_time_;
        double seconds = elapsed.seconds();
        
        if (seconds > 0.0)
        {
            // 计算帧率
            double fps = static_cast<double>(frame_count_) / seconds;
            RCLCPP_INFO(this->get_logger(), "相机帧率: %.2f FPS", fps);
            
            // 重置计数器和时间
            frame_count_ = 0;
            last_time_ = current_time;
        }
    }
    
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Time last_time_;
    int frame_count_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<FramerateMonitor>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

